#include "controller_3D_posPID.h"


controller_3D_posPID_c::
controller_3D_posPID_c(float dT)
{

	ref.position.x	= 0;
	ref.position.y	= 0;
	ref.position.z  = 0;
	ref.yaw			= 0;

	feedback.position.x = 0;
	feedback.position.y = 0;
	feedback.position.z = 0;
	feedback.yaw		= 0;

	controller_posPID_c controller_x(dT);
	controller_posPID_c controller_y(dT);
	controller_posPID_c controller_z(dT);

	d3_ctrl.velocity.vx = 0;
	d3_ctrl.velocity.vy = 0;
	d3_ctrl.velocity.vz = 0;
	d3_ctrl.yaw_ref 	= 0;
	
}

controller_3D_posPID_c::
controller_3D_posPID_c()
{

	ref.position.x	= 0;
	ref.position.y	= 0;
	ref.position.z  = 0;
	ref.yaw			= 0;

	feedback.position.x = 0;
	feedback.position.y = 0;
	feedback.position.z = 0;
	feedback.yaw		= 0;

	controller_posPID_c controller_x(1);
	controller_posPID_c controller_y(1);
	controller_posPID_c controller_z(1);

	d3_ctrl.velocity.vx = 0;
	d3_ctrl.velocity.vy = 0;
	d3_ctrl.velocity.vz = 0;
	d3_ctrl.yaw_ref 	= 0;
	
}

controller_3D_posPID_c::
~controller_3D_posPID_c()
{}


void 
controller_3D_posPID_c::
set(d3_PID_t d3_pid)
{
	controller_x.set(d3_pid.PID_x.Kp, d3_pid.PID_x.Ki, d3_pid.PID_x.Kd, d3_pid.PID_x.u_limit, d3_pid.PID_x.l_limit);
	controller_y.set(d3_pid.PID_y.Kp, d3_pid.PID_y.Ki, d3_pid.PID_y.Kd, d3_pid.PID_y.u_limit, d3_pid.PID_y.l_limit);
	controller_z.set(d3_pid.PID_z.Kp, d3_pid.PID_z.Ki, d3_pid.PID_z.Kd, d3_pid.PID_z.u_limit, d3_pid.PID_z.l_limit);
}


int*
controller_3D_posPID_c::
calc()
{
	int a[3] = {0};
	int* d3_windup = a;
	controller_x.ref 		= ref.position.x;
	controller_x.feedback 	= feedback.position.x;
	a[0] = controller_x.calc();
	d3_ctrl.velocity.vx 	= controller_x.u_ctrl;

	controller_y.ref 		= ref.position.y;
	controller_y.feedback 	= feedback.position.y;
	a[1] = controller_y.calc();
	d3_ctrl.velocity.vy 	= controller_y.u_ctrl;

	controller_z.ref 		= ref.position.z;
	controller_z.feedback 	= feedback.position.z;
	a[2] = controller_z.calc();
	d3_ctrl.velocity.vz 	= controller_z.u_ctrl;	
	//printf("controller_z.ref = %.2f\n", controller_z.ref);
	//printf("controller_z.feedback = %.2f\n", controller_z.feedback);
	//printf("d3_ctrl.velocity.vz = %.2f\n", d3_ctrl.velocity.vz);

	d3_ctrl.yaw_ref			= ref.yaw;

	return d3_windup;
}

void 
controller_3D_posPID_c::
set_yaw(float yaw_degree)			//in degree
{
	d3_ctrl.yaw_ref = yaw_degree / 180 * PI;
}

